package nl.nhl.idp.Components.Autonomous;

/**
 * Engine to simulate autonomous behaviour for the IDP Amphibious vehicle.
 * This class is merely a dummy class, it should be further implemented.
 * 
 * Idea: when front is blocked, determine which direction to turn. Save this direction and start turning.
 * As soon as the sensors indicate the turn has been completed, set direction to forward again.
 * turn is completed when sensorreading for forward approximate the sensor values that have been saved before the turn.
 * @author Zarlin
 */

public class AutonomousEngine
{
	//the value corresponding with no vehicle movement/steering
	private final int ZERO_VALUE = 127;
	
	//sensor value DISTANCE_THRESHOLD, if lower, a wall is 'too close', if higher, the way is free
	private final int DISTANCE_THRESHOLD = 100;
	
	//defines how intense vehicle movements are, lower values makes the vehicle move slower and more careful
	private final int MOVE_INTENSITY = 20;
	
	private int drive;
	private int steer;
	
	public int GetDrive()
	{
		return drive;
	}
	
	public int GetSteer()
	{
		return steer;
	}
	
	
	/**
	 * Create new AutonomousEngine
	 */
	public AutonomousEngine()
	{
	}
	
	/**
	 * Set current sensor values so the AutonomousEngine can calculate a command
	 * @param sensorFront
	 * @param sensorRight
	 * @param sensorLeft
	 */
	public void SetSensors(int sensorFront, int sensorRight, int sensorLeft)
	{
		//if the way ahead is free, move forward
		if(sensorFront > DISTANCE_THRESHOLD)
		{
			drive = ZERO_VALUE+MOVE_INTENSITY;
			steer = ZERO_VALUE;
		}
		else
		{
			//TODO: Needs testing and probably expansion
			
			//if the way ahead is blocked, but the right is open, turn right
			if(sensorRight > DISTANCE_THRESHOLD)
			{	
				drive = ZERO_VALUE;
				steer = ZERO_VALUE+MOVE_INTENSITY;
			}
			
			//if forward and right are blocked, try turning left
			else if(sensorLeft > DISTANCE_THRESHOLD)
			{				
				drive = ZERO_VALUE;
				steer = ZERO_VALUE-MOVE_INTENSITY;
			}	
		}
	}
}